#include "CweldCalculator.h"

#include <iostream>
#include <sstream>

#include "Utility/Logging/SGLogger.h"
#include "Utility/Algebra/MathTools.h"
#include "Utility/Algebra/Matrix.h"
#include "Utility/Algebra/Tools.h"

#include "DataStructure/Input/Material/IsotropicMaterial.h"
#include "DataStructure/Input/Node/NodeData.h"
#include "DataStructure/Input/Property/PBARData.h"
#include "DataStructure/Input/Property/PWELDData.h"
#include "DataStructure/Input/Element/CWELDData.h"

#include "DBManager/DBServiceFactory.h"
#include "DBManager/IService/Input/Property/IPWELDService.h"
#include "DBManager/IService/Input/Element/IElementService.h"
#include "DBManager/IService/Input/Material/IIsotropicMaterialService.h"
#include "DBManager/IService/Input/Node/INodeService.h"


#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/CoordinateProcessor.h"
#include "SGFem/Pre/Assembly/ModelInfoQuery.h"
#include "SGFem/Integration/ShapeFunction/ParaCoordCalctor.h"
#include "SGFem/Integration/ShapeFunction/Plane4NodeShpFunc.h"

#include "SGFem/ElementCalculator/BarElement/BarCalculator.h"
#include "SGFem/BeamSecPropCalculator/FormulaMethod/Bar/RodSectionFormulaCalculator.h"
#include "SGFem/ElementCalculator/Common/ElementTools.hpp"
#include "SGFem/ConnectElement/CweldElement/ConnectElementHleper.h"


using SG::Algebra::Matrix;
using SG::Algebra::Matrixd;
using SG::Algebra::Point;
using SG::Algebra::PointArray;
using SG::Algebra::Vector3D;
using SG::DataStructure::Common::Status;
using SG::DataStructure::Common::Id;
using SG::DataStructure::Common::IdArray;
using SG::DataStructure::Common::Real;
using SG::DataStructure::Common::TOLERANCE;
using namespace SG::DataStructure::FEM;
using namespace SG::DBManager;

using SG::DataStructure::FEM::IntegPointArray;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::CoordinateProcessor;
using SG::FEMSolver::Element::BarCalculator;
using SG::FEMSolver::Element::CweldCalculator;
using SG::FEMSolver::Element::ConnectElementHleper;

CweldCalculator::CweldCalculator ()
{}

void CweldCalculator::ComputeSurfaceCoord (const SG::Algebra::PointArray& gSet, 
                                           _OUT SG::Algebra::Matrix&      T, 
                                           _OUT SG::Algebra::Point&       origin, 
                                           _OUT SG::Algebra::Matrix&      localCoord)
{
    switch (gSet.size())
    {
    case 3:
        {
            ElementTools::computePlaneCoordTrans(gSet, _OUT T, _OUT origin, _OUT localCoord);
            break;
        }
    case 4:
        {
            ElementTools::computeQaud4CoordTrans(gSet, _OUT T, _OUT origin, _OUT localCoord);
            break;
        }
    case 6:
        {
            PointArray gSet2(3);
            gSet2[0] = gSet[0];
            gSet2[1] = gSet[1];
            gSet2[2] = gSet[2];
            ElementTools::computePlaneCoordTrans(gSet2, _OUT T, _OUT origin, _OUT localCoord);
            break;
        }
    case 8:
        {
            PointArray gSet2(4);
            gSet2[0] = gSet[0];
            gSet2[1] = gSet[1];
            gSet2[2] = gSet[2];
            gSet2[3] = gSet[3];
            ElementTools::computeQaud4CoordTrans(gSet2, _OUT T, _OUT origin, _OUT localCoord);
            break;
        }
    default:
        break;
    }
}

void CweldCalculator::FindNearestElement
(
    const DBServiceFactorySP dbServiceFactory,
    const SG::Algebra::Point& point,
    const SG::DataStructure::Common::IdArray& ShellGroup,
    _OUT SG::DataStructure::Common::Id& TargetShellId
)
{
    const auto& eleSummaryRepo = dbServiceFactory->get<IElementService> ();
    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();
    PointArray                         gASet;
    std::vector<std::size_t>           nodeIndexSetA;
    SG::DataStructure::Common::IdArray dispCoordsA;

    double minDistance = std::numeric_limits<double>::max();
    for (size_t i = 0; i < ShellGroup.size(); ++i)
    {
        auto nodelist = eleSummaryRepo ->FindElementNodes (ShellGroup[i]);
        ModelInfoQuery::QueryNode (nodeRepository, nodelist.data(), nodelist.size (), gASet, nodeIndexSetA, dispCoordsA);
        Point PointMid;
        if (gASet.size() == 3)
        {
            PointMid = {(gASet[0].m_x + gASet[1].m_x + gASet[2].m_x)/3, (gASet[0].m_y + gASet[1].m_y + gASet[2].m_y)/3, (gASet[0].m_z + gASet[1].m_z + gASet[2].m_z)/3};
        }
        if (gASet.size() == 4)
        {
            PointMid = {(gASet[0].m_x + gASet[2].m_x)/2, (gASet[0].m_y + gASet[2].m_y)/2, (gASet[0].m_z + gASet[2].m_z)/2};
        }
        Real distance = length(point, PointMid);
        if (distance < minDistance)
        {
            minDistance = distance;
            TargetShellId = ShellGroup[i];
        }
    }
}

void CweldCalculator::ComputeProjectionPoint2 (const SG::Algebra::Point                gS,
                                               const SG::Algebra::PointArray&          gASet,
                                               const SG::Algebra::PointArray&          gBSet,
                                               SG::Algebra::Point&                     GA,
                                               SG::Algebra::Point&                     GB,
                                               SG::Algebra::Vector3D&                  nA)
{
    // Task1：求两个面的法向量(分3/4/6/8节点)
    Matrix localCoord;
    Matrix TA, TB;
    Point  originA, originB;

    CweldCalculator::ComputeSurfaceCoord (gASet, _OUT TA, _OUT originA, _OUT localCoord);
    nA = { TA (0, 2), TA (1, 2), TA (2, 2) };

    CweldCalculator::ComputeSurfaceCoord (gBSet, _OUT TB, _OUT originB, _OUT localCoord);
    Vector3D nB = { TB (0, 2), TB (1, 2), TB (2, 2) };

    // Task2：计算平面 Ax+By+Cz+D=0 的常数 DA DB
    auto DA = -(TA (0, 2) * gASet[0].m_x + TA (1, 2) * gASet[0].m_y + TA (2, 2) * gASet[0].m_z);
    auto DB = -(TB (0, 2) * gBSet[0].m_x + TB (1, 2) * gBSet[0].m_y + TB (2, 2) * gBSet[0].m_z);

    // Task3：将GS点代入两个平面的方程，DA = A1X1+B1Y1+C1Z1+D1, DB=A2X2+B2Y2+C2Z2+D2，
    auto D1 = (TA (0, 2) * gS.m_x + TA (1, 2) * gS.m_y + TA (2, 2) * gS.m_z + DA);
    auto D2 = (TB (0, 2) * gS.m_x + TB (1, 2) * gS.m_y + TB (2, 2) * gS.m_z + DB);

    // Task4：根据几何条件判断GS点在平面内外，并根据结果做1次或2次投影。
    // Task4.1：过GS作面A和面B的一次投影到GA'和GB'。（即上面求出的pointGA、pointGB）
    projectPoint2Face (originA, nA, gS, _OUT GA);
    projectPoint2Face (originB, nB, gS, _OUT GB);

    // Task4.2：若DA*DB>0，则GS点在A和B面之外。需进行二次投影。
    if ((dot(nA,nB) > 0 && D1 * D2 > 0) || (dot(nA,nB) < 0 && D1 * D2 < 0))
    {
        // Task4.2.1：连接GA'和GB'，求出中点GC。
        Point pointGC{ 0.5 * (GA.m_x + GB.m_x), 0.5 * (GA.m_y + GB.m_y), 0.5 * (GA.m_z + GB.m_z) };
        // Task4.2.2：过GC作面A和面B的二次投影到GA和GB。
        projectPoint2Face (originA, nA, pointGC,_OUT GA);
        projectPoint2Face (originB, nB, pointGC,_OUT GB);
    }
}

/** @brief   建立参考面节点全局坐标系 和投影点basic坐标下位移约束方程
 *
 * @param         Gi            投影点全局坐标   
 * @param         planeGlobal   壳面节点全局坐标
 * @param         dispCoordset  平面连接节点的位移输出坐标系
 * @param[out]    R             约束方程 s.t. uProj = R*u_Shell 
 * @author   yangqiuyou
 * @date     2025.05.15
 */
void CweldCalculator::ComputeConstrantEquation (const SG::Algebra::Point& Gi, const Id& projectdispCoodId,  const SG::Algebra::PointArray& planeGlobal, const SG::DataStructure::Common::IdArray& dispCoordset, SG::Algebra::Matrixd& _OUT R)
{
    // 将 Point 变换到 shell的单元局部坐标系下
    Matrix T(3, 3);
    auto nNode = planeGlobal.size();
    Matrix R1(6, 3 * nNode);
    switch(nNode)
    {
        case 3:
        {
            CweldCalculator::TriConstrantEquation (Gi, planeGlobal, _OUT T, _OUT R1);
            break;
        }
        case 4:
        {
            CweldCalculator::QuadConstrantEquation (Gi, planeGlobal, _OUT T, _OUT R1);
            break;
        }
        default :
        {
            std::cout<<"Error: The number of nodes is not 3 or 4!"<<std::endl;
            break;
        }
    };

    // 投影点全局到局部的转换矩阵，对角矩阵
    SG::Algebra::Matrix T1 = Matrix (6, 6);
    T1.BlockFill (0, 0, T);
    T1.BlockFill (3, 3, T);
    
    // 节点处位移输出坐标系
    SG::Algebra::Matrix T2 = Matrix (3 * nNode, 3 * nNode);
    const auto& coordProcessor = CoordinateProcessor::GetInstance ();
    for (std::size_t size = 0; size < nNode; size++)
    {   
        if (dispCoordset[size] > 0)
        {
            Matrixd Ti;
            coordProcessor.GetCoordTrans (dispCoordset[size], planeGlobal[size], _OUT Ti);
            T2.BlockFill (3 * size, 3 * size, (Ti*T).Transpose());
        }
        else
        {
            T2.BlockFill (3 * size, 3 * size, T.Transpose());
        }
    }

    R = T1 * R1 * T2;

    //判断是否需要把辅助点位移转换到basic坐标系下
    if(0 != projectdispCoodId)
    {
        Matrixd Tg;
        coordProcessor.GetCoordTrans (projectdispCoodId, Gi, _OUT Tg);
        // Tg = Tg.Transpose();
        auto Tm = Matrix (6, 6);
        Tm.BlockFill (0, 0, Tg);
        Tm.BlockFill (3, 3, Tg);
        R = Tm * R;
    }
}

/** @brief        求解三角形约束方程
 *
 * @param         Gi            投影点全局坐标   
 * @param         planeGlobal   壳面节点全局坐标
 * @param[out]    T             壳面转换矩阵
 * @param[out]    R             R*[u_A; u_Shell_G] = 0 约束方程
 * @author   yangqiuyou
 * @date     2025.05.15
 */
void CweldCalculator::TriConstrantEquation ( const SG::Algebra::Point&                 Gi,
                                             const SG::Algebra::PointArray&            planeGlobal,
                                             SG::Algebra::Matrixd& _OUT                T,
                                             SG::Algebra::Matrixd& _OUT                R)
{
    T = Matrix(3, 3); 
    Matrix planeLocal;
    Point  origin;
    ElementTools::computePlaneCoordTrans (planeGlobal, _OUT T, _OUT origin, _OUT planeLocal);
    
    const Point coord1 = {planeLocal(0, 0), planeLocal(0, 1), 0.0};
    const Point coord2 = {planeLocal(1, 0), planeLocal(1, 1), 0.0};
    const Point coord3 = {planeLocal(2, 0), planeLocal(2, 1), 0.0};

    Point pGLocal;
    Point::Transform(T.Transpose(), origin, Gi, _OUT pGLocal);

    Vector3D normal;
    const Real A  = SG::Algebra::calc2DTriArea(planeLocal , normal);
    const Real A1 = SG::Algebra::calcTriArea(coord1, coord2, pGLocal, normal);
    const Real A2 = SG::Algebra::calcTriArea(coord1, coord3, pGLocal, normal);
    
    const auto N2 = A2 / A;
    const auto N3 = A1 / A;
    const auto N1 = 1 - N2 - N3;
    
    //对x,y的偏导数
    auto det = A * 2;
    const auto N1x = (coord2.m_y - coord3.m_y)/det;
    const auto N1y = (coord3.m_x - coord2.m_x)/det;
    const auto N2x = (coord3.m_y - coord1.m_y)/det;
    const auto N2y = (coord1.m_x - coord3.m_x)/det;
    const auto N3x = (coord1.m_y - coord2.m_y)/det;
    const auto N3y = (coord2.m_x - coord1.m_x)/det;
    
    R = Matrixd (6 , 9);
    R (0, 0) =  N1,      R (0, 3) = N2,         R (0, 6) = N3;
    R (1, 1) =  N1,      R (1, 4) = N2,         R (1, 7) = N3;
    R (2, 2) =  N1,      R (2, 5) = N2,         R (2, 8) = N3;
    R (3, 2) =  N1y,     R (3, 5) = N2y,        R (3, 8) = N3y;
    R (4, 2) = -N1x,     R (4, 5) = -N2x,       R (4, 8) = -N3x;
    R (5 ,0) = -N1y*0.5, R (5, 3) = -N2y * 0.5, R (5, 6) = -N3y * 0.5;
    R (5 ,1) =  N1x*0.5, R (5, 4) =  N2x * 0.5, R (5, 7) = N3x  * 0.5;
}

/** @brief       求解四边形约束方程
*
* @param         Gi            投影点全局坐标   
* @param         planeGlobal   壳面节点全局坐标
* @param[out]    T             壳面转换矩阵
* @param[out]    R             R*[u_A; u_Shell_G] = 0 约束方程
* @author   yangqiuyou
* @date     2025.05.15
*/
void CweldCalculator::QuadConstrantEquation (const SG::Algebra::Point&                 Gi,
                                             const SG::Algebra::PointArray&            planeGlobal,
                                             SG::Algebra::Matrixd& _OUT                T,
                                             SG::Algebra::Matrixd& _OUT                R)
{
    T = Matrix(3, 3); 
    Matrix planeLocal;
    Point  origin;
    ElementTools::computeQaud4CoordTrans (planeGlobal, _OUT T, _OUT origin, _OUT planeLocal);

    Point pGLocal;
    Point::Transform(T.Transpose(), origin, Gi, _OUT pGLocal);
    Matrix pointLocal (1, 2, {pGLocal.m_x, pGLocal.m_y});

    //四节点对应的约束方程
    IntegPointArray integ (1);
    SG::FEMSolver::Element::quad4ComputeIntegCoord (pointLocal, planeLocal, _OUT integ[0].m_x, _OUT integ[0].m_y);
    
    Plane4NodeShpFunc shapeFunc;
    shapeFunc.Compute (integ);
    const Matrix DN = shapeFunc.GetShapeFunctionDerivative (0);
    auto J = DN * planeLocal;
    auto Jinev = SG::Algebra::inverse2X2 (J);
    auto DNDx = Jinev * DN;
    
    const auto& N  = shapeFunc.GetShapeFunction (0);
    const auto& N1 = N (0,0);
    const auto& N2 = N (1,0);
    const auto& N3 = N (2,0);
    const auto& N4 = N (3,0);
    
    //对x,y的偏导数
    Real N1x, N2x, N3x, N4x;
    Real N1y, N2y, N3y, N4y;
    N1x = DNDx (0, 0), N2x = DNDx (0, 1), N3x = DNDx (0, 2), N4x = DNDx (0, 3);
    N1y = DNDx (1, 0), N2y = DNDx (1, 1), N3y = DNDx (1, 2), N4y = DNDx (1, 3);
    
    R = Matrixd (6 , 12);
    R (0, 0) = N1,       R (0, 3) = N2,        R (0, 6) = N3,       R (0, 9) = N4;
    R (1, 1) = N1,       R (1, 4) = N2,        R (1, 7) = N3,       R (1, 10) = N4;
    R (2, 2) = N1,       R (2, 5) = N2,        R (2, 8) = N3,       R (2, 11) = N4;
    R (3, 2) = N1y,      R (3, 5) = N2y,       R (3, 8) = N3y,      R (3, 11) = N4y;
    R (4, 2) = -N1x,     R (4, 5) = -N2x,      R (4, 8) = -N3x,     R (4, 11) = -N4x;
    R (5, 0) = -N1y / 2, R (5, 3) = -N2y / 2,  R (5, 6) = -N3y / 2, R (5, 9) = -N4y / 2;
    R (5, 1) = N1x / 2,  R (5, 4) = N2x / 2,   R (5, 7) = N3x / 2,  R (5, 10) = N4x / 2;
}

void CweldCalculator::Initialize (SG::DataStructure::FEM::ElementTypes type)
{
    // do nothing
}

/** @brief  当投影点重合时，特殊的刚度矩阵（类似BUSH单元） */
void CweldCalculator::ComputeStiffness0 (const Real& L, const IsotropicMaterial& material, const PBARData& property, Matrixd& _OUT kLocal)
{
    const Real& E{ material.m_E };
    const Real& G{ material.m_G };
    const Real& A{ property.m_A };

    const Real EAL{ E * A / L };
    const Real GAL{ G * A / L };
    const Real EIL{ E * property.m_I1 / L };
    const Real GJL{ G * property.m_J / L };

    Matrixd k1 (6, 6);
    k1 (0, 0) = EAL;
    k1 (1, 1) = GAL;
    k1 (2, 2) = GAL;
    k1 (3, 3) = GJL;
    k1 (4, 4) = EIL;
    k1 (5, 5) = EIL;

    kLocal.BlockFill (0, 0, k1);
    kLocal.BlockFill (6, 6, k1);
    kLocal.BlockFill (0, 6, k1 * -1.0);
    kLocal.BlockFill (6, 0, k1 * -1.0);
}

/** @brief 修正的梁刚度矩阵 (无弯曲梁) */
void CweldCalculator::ComputeStiffness1 (const Real& L, const IsotropicMaterial& material, const PBARData& property, _OUT Matrixd& kl)
{
    const Real& E{ material.m_E };
    const Real& G{ material.m_G };
    const Real& A{ property.m_A };

    kl = Matrixd (12, 12);
    const Real EAL{ E * A / L };
    kl (0, 0) = EAL;
    kl (0, 6) = -EAL;
    kl (6, 0) = -EAL;
    kl (6, 6) = EAL;

    // Torsion
    const Real GJL{ G * property.m_J / L };
    kl (3, 3) = GJL;
    kl (3, 9) = -GJL;
    kl (9, 3) = -GJL;
    kl (9, 9) =  GJL;

    const Real EI1{E * property.m_I1};
    const Real EI2{E * property.m_I2};
    const Real R{G * A / L};

    // Plane 1
    kl (1, 1) = R;           kl (1, 5) = 0.5*R*L;            kl (1, 7) = -R;          kl (1, 11) = 0.5*R*L;
    kl (5, 1) = kl (1, 5);   kl (5, 5) = 0.25*R*L*L + EI1/L; kl (5, 7) = -0.5*R*L;    kl (5, 11) = 0.25*R*L*L - EI1/L;
    kl (7, 1) = kl (1, 7);   kl (7, 5) = kl (5, 7);          kl (7, 7) = R;           kl (7, 11) = -0.5*R*L;
    kl (11, 1) = kl (1, 11); kl (11, 5) = kl (5, 11);        kl (11, 7) = kl (7, 11); kl (11, 11) = 0.25*R*L*L + EI1/L;

    // Plane 2
    kl (2, 2) = R;           kl (2, 4) = -0.5*R*L;           kl (2, 8) = -R;          kl (2, 10) = -0.5*R*L;
    kl (4, 2) = kl (2, 4);   kl (4, 4) = 0.25*R*L*L + EI2/L; kl (4, 8) = 0.5*R*L;     kl (4, 10) = 0.25*R*L*L - EI2/L;
    kl (8, 2) = kl (2, 8);   kl (8, 4) = kl (4, 8);          kl (8, 8) = R;           kl (8, 10) = 0.5*R*L;
    kl (10, 2) = kl (2, 10); kl (10, 4) = kl (4, 10);        kl (10, 8) = kl (8, 10); kl (10, 10) = 0.25*R*L*L + EI2/L;
};

void CweldCalculator::ComputeElementMatries (const DBServiceFactorySP dbServiceFactory)
{
    auto&       coordHandler   = CoordinateProcessor::GetInstance ();
    auto&       assembly       = AssemblyEigen::GetInstance ();
    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();
    const auto  eleRepository  = dbServiceFactory->get<IElementService> ();

    // 截面计算，获取厚度
    ConnectElementHleper connectHleper;

    static const DofSet dofSet{ DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz,
                                DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz };
    static const DofSet TransdofSet{ DOFDirections::DOF_Ux, DOFDirections::DOF_Uy, DOFDirections::DOF_Uz };

    std::vector<int>         dofGlobalIndexSet (12);
    std::vector<std::size_t> nodeIndexSet (2);
    // 单元循环
    SG::DataStructure::FEM::CWELDData eleData;
    Matrixd   kg (12, 12);

    PointArray                         gASet;
    PointArray                         gBSet;
    std::vector<std::size_t>           nodeIndexSetA;
    std::vector<std::size_t>           nodeIndexSetB;
    SG::DataStructure::Common::IdArray dispCoordsA;
    SG::DataStructure::Common::IdArray dispCoordsB;

    auto      eleSize = eleRepository->Size<SG::DataStructure::FEM::CWELDData>();
    for (std::size_t index{ 0 }; index < eleSize; ++index)
    {
        eleRepository->FindByIndex (index, _OUT eleData);

        // 连接节点的Basic坐标 及 位移输出坐标系
        Point    GA , GB;
        Id dispCoordGA{0};
        Id dispCoordGB{0};

        // 长度为0.0，需要使用壳厚度
        Real tA{0.0};
        Real tB{0.0};
        Vector3D nA,nB;
        Point    pointGS;
        Id       dispCoordsGS;
        NodeData node;
        // double distance;

        // 获取投影节点
        if (eleData.m_Gs > 0)
        {
            nodeRepository ->FindById (eleData.m_Gs, _OUT node);
            coordHandler.GetGlobalCoordinate (node, _OUT pointGS, _OUT dispCoordsGS);
        }
        // 如果指定了GA和GB，先读取GA和GB的数据。
        if (eleData.m_g[0] < 101000000 && eleData.m_g[1] < 101000000)
        {
            nodeRepository ->FindById (eleData.m_g[0], _OUT node);
            coordHandler.GetGlobalCoordinate (node, _OUT GA, _OUT dispCoordGA);
            nodeRepository ->FindById (eleData.m_g[1], _OUT node);
            coordHandler.GetGlobalCoordinate (node, _OUT GB, _OUT dispCoordGB);
        }

        auto weldFormat = eleData.GetFormat();
        switch (weldFormat)
        {
            //Task1.1 如果是ElemId, 获取GS, SHIDA, SHIDB, 计算投影点。
            case CWELDFormats::FORMAT_ElemID:
            {
                // 利用壳单元编号 ShId 获得节点id
                auto       nodelist = eleRepository ->FindElementNodes (eleData.m_ShIdA);
                ModelInfoQuery::QueryNode (nodeRepository, nodelist.data(), nodelist.size (), gASet, nodeIndexSetA, dispCoordsA);

                // Task1.1.1 如果同时给定SHIDA和SHIDB。面对面情况。
                if (0 != eleData.m_ShIdB)
                {
                    nodelist = eleRepository ->FindElementNodes (eleData.m_ShIdB);
                    ModelInfoQuery::QueryNode (nodeRepository, nodelist.data(), nodelist.size (), gBSet, nodeIndexSetB, dispCoordsB);

                    // 通过GS重新计算 投影点坐标
                    if (eleData.m_Gs > 0)
                    {
                        ComputeProjectionPoint2 (pointGS, _OUT gASet, _OUT gBSet, _OUT GA, _OUT GB, _OUT nA);
                    }

                    // 查询壳厚度， 节点重合特别处理
                    Real L = length (GA, GB);
                    if (L < 1.0e-6)
                    {                        
                        // 查询壳截面属性Id
                        auto pIdA = eleRepository -> QueryShellPropId (eleData.m_ShIdA);
                        tA = connectHleper.ComputeShellSecThickness (dbServiceFactory, pIdA);

                        // 另一个截面
                        auto pIdB = eleRepository -> QueryShellPropId (eleData.m_ShIdB);
                        tB = (pIdA == pIdB) ? tA : connectHleper.ComputeShellSecThickness (dbServiceFactory, pIdB);
                    }

                    break;
                }

                // Task1.1.2 只给定SHIDA和GS点。点对面情况。
                Matrix localCoord;
                Point origin;
                Matrix TA;

                CweldCalculator::ComputeSurfaceCoord (gASet, _OUT TA, _OUT origin, _OUT localCoord);
                nA = { TA (0, 2), TA (1, 2), TA (2, 2) };
                projectPoint2Face (origin, nA, pointGS, _OUT GA);

                // Gs 直接与 面A相连
                GB = pointGS;
                dispCoordGB = dispCoordsGS;

                // 是否要查询PIDA 和 PIDB
                break;
            }
            //Task1.2 如果是GridId, 获取GS, GAI, GBI, 计算投影点。
            case CWELDFormats::FORMAT_GridID:
            {
                auto NUpper = eleData.GetGUpperSize();
                ModelInfoQuery::QueryNode (nodeRepository, eleData.m_GUpper, NUpper, gASet, nodeIndexSetA, dispCoordsA);
                auto NLower = eleData.GetLowerSize();
                ModelInfoQuery::QueryNode (nodeRepository, eleData.m_GLower, NLower, gBSet, nodeIndexSetB, dispCoordsB);
                ComputeProjectionPoint2(pointGS, gASet, gBSet, GA, GB, nA);
                break;
            }
            // //Task1.3 如果是PartPat, 获取GS, PIDA, PIDB, 计算投影点。
            case CWELDFormats::FORMAT_PartPat:
            {
                // Task1.3.1 利用哈希表找到PID符合的单元。                
                IdArray validShellIdsA = eleRepository->FindElementByPropId (eleData.m_PIdA);
                IdArray validShellIdsB = eleRepository->FindElementByPropId (eleData.m_PIdB);
           
                // Task1.3.2 计算GS点到其中距离最小的单元。
                Id ShellIdA, ShellIdB;
                FindNearestElement(dbServiceFactory, pointGS, validShellIdsA, _OUT ShellIdA);
                FindNearestElement(dbServiceFactory, pointGS, validShellIdsB, _OUT ShellIdB);
                
                // Task1.3.3 进行投影
                auto       nodelist = eleRepository ->FindElementNodes (ShellIdA);
                ModelInfoQuery::QueryNode (nodeRepository, nodelist.data(), nodelist.size (), gASet, nodeIndexSetA, dispCoordsA);
                nodelist = eleRepository ->FindElementNodes (ShellIdB);
                ModelInfoQuery::QueryNode (nodeRepository, nodelist.data(), nodelist.size (), gBSet, nodeIndexSetB, dispCoordsB);
                ComputeProjectionPoint2 ( pointGS,  _OUT gASet, _OUT gBSet, _OUT GA, _OUT GB, _OUT nA);
                break;
            }
            // //Task1.4 如果是ElPat, 获取GS, SHIDA, SHIDB, 计算投影点。
            case CWELDFormats::FORMAT_ElPat:
            {
                auto       nodelist = eleRepository ->FindElementNodes (eleData.m_ShIdA);
                ModelInfoQuery::QueryNode (nodeRepository, nodelist.data(), nodelist.size (), gASet, nodeIndexSetA, dispCoordsA);
                nodelist = eleRepository ->FindElementNodes (eleData.m_ShIdB);
                ModelInfoQuery::QueryNode (nodeRepository, nodelist.data(), nodelist.size (), gBSet, nodeIndexSetB, dispCoordsB);
                ComputeProjectionPoint2 (pointGS,  gASet, gBSet, _OUT GA, _OUT GB, _OUT nA);
                break;
            }
            // //Task1.5 如果是Align, 直接获取GA, GB。
            case CWELDFormats::FORMAT_ALign:
            {
                nodeRepository ->FindById (eleData.m_g[0], _OUT node);
                coordHandler.GetGlobalCoordinate (node, GA, dispCoordGA);
                nodeRepository ->FindById (eleData.m_g[1], _OUT node);
                coordHandler.GetGlobalCoordinate (node, GB, dispCoordGB);
                break;
            }
            default:
                break;
        }

        //Task2：输出投影点 GA,GB
        Real L = length (GA, GB);
        const bool zeroLenCase = (L < 1.0e-6);
        Real Le;   //等效长度
        // compute beam section property
        PBARData         pBar;
        IsotropicMaterial material;
        {
            PWELDData        pWeld;
            dbServiceFactory->get<IPWELDService> () ->FindById (eleData.m_pId, pWeld);
            dbServiceFactory->get<IIsotropicMaterialService> ()->FindById (pWeld.m_MID, _OUT material);
            
            // create beam section data
            PBARLData    rodSec;
            rodSec.m_id = eleData.m_pId;
            rodSec.m_dimension = {0.5 * pWeld.m_D};
            // 节点重合情况
            if (zeroLenCase)
            {
                // 获取壳截面厚度
                Le =(tA + tB) / 2.0;
            }
            Point centroid;
            auto  calculator = SG::FEMSolver::Element::RodSectionFormulaCalculator ();
            calculator.Compute (rodSec, _OUT pBar, _OUT centroid);
        }
        
        // 粱单元刚度矩阵计算
        Matrix  T (3, 3);
        if (zeroLenCase)
        {
            // 梁长为零的特殊情况
            Point point1{0.0, 0.0, 0.0};
            Point point2{nA.m_x, nA.m_y, nA.m_z};
            SG::Algebra::computeLocalCoordTrans (point1, point2, _OUT T);
            ComputeStiffness0 (Le, material, pBar, _OUT kg);
        }
        else
        {
            SG::Algebra::computeLocalCoordTrans (GA, GB, _OUT T);
            ComputeStiffness1 (L, material, pBar, _OUT kg);
        }
        
        Matrixd TBeam = ElementTools::TwoNodeEleCoordTrans (T, dispCoordGA, dispCoordGB, GA, GB);
        kg = TBeam.Transpose () * kg * TBeam;

        // assembly
        assembly.FindNodeIndex (eleData.m_g, _OUT nodeIndexSet);
        assembly.GetGlobalDOFId (nodeIndexSet, dofSet, _OUT dofGlobalIndexSet);
        assembly.AccumulateStiffness (dofGlobalIndexSet, kg);
        
        std::vector<int> mSetGlobalId (6);
        std::vector<int> nSetGlobalId (12);
        Matrix RnRm;
        Id dispId = 0;
        if(!gASet.empty())
        {
            CweldCalculator::ComputeConstrantEquation (GA, dispId, gASet, dispCoordsA, RnRm);

            assembly.GetGlobalDOFId ({ nodeIndexSet[0] }, dofSet, _OUT mSetGlobalId);
            assembly.GetGlobalDOFId (nodeIndexSetA, TransdofSet, _OUT nSetGlobalId);
            // nSetGlobalId.insert (nSetGlobalId.end (), mSetGlobalId.begin (), mSetGlobalId.end ());
            assembly.AccumulateControlMatrix (nSetGlobalId, mSetGlobalId, RnRm);
            for (auto& gdofm : mSetGlobalId)
            {
                assembly.AccumulateControlMatrix(gdofm, gdofm, -1.0);
            }

        }

        if(!gBSet.empty())
        {
            CweldCalculator::ComputeConstrantEquation (GB, dispId, gBSet,dispCoordsB, RnRm);

            assembly.GetGlobalDOFId ({ nodeIndexSet[1] }, dofSet, _OUT mSetGlobalId);
            assembly.GetGlobalDOFId (nodeIndexSetB, TransdofSet, _OUT nSetGlobalId);
            // nSetGlobalId.insert (nSetGlobalId.end (), mSetGlobalId.begin (), mSetGlobalId.end ());
            assembly.AccumulateControlMatrix (nSetGlobalId, mSetGlobalId, RnRm);
            for (auto& gdofm : mSetGlobalId)
            {
                assembly.AccumulateControlMatrix(gdofm, gdofm, -1.0);
            }
        }
    }
}

void CweldCalculator::DataRecovery (const SG::DBManager::DBServiceFactorySP             dbServiceFactory,
                                    int                                                 subcaseId,
                                    SG::DataStructure::Common::Real                     time,
                                    const std::vector<SG::DataStructure::Common::Real>& globalDisplacement)
{
    // SG::Algebra::Matrixd elementInternalForce;
    // ComputeElementInternalForce(dbServiceFactory, globalDisplacement, elementInternalForce);
}

void CweldCalculator::ComputeStressStiffness (const SG::DBManager::DBServiceFactorySP dbServiceFactory, int staticSubId)
{}

void CweldCalculator::ComputeElementInternalForce(const SG::DBManager::DBServiceFactorySP             dbServiceFactory,
                                                  const std::vector<SG::DataStructure::Common::Real>& globalDisplacement,
                                                  _OUT SG::Algebra::Matrixd&                          elementInternalForce)
{
//     1. 通过GA GB 节点的全局自由度编号，获取两个节点各自的6个位移分量【此时位移分量位于下】
    auto& coordHandler   = CoordinateProcessor::GetInstance ();
    auto& assembly = AssemblyEigen::GetInstance ();
    std::vector<std::size_t> nodeIndexSet (2);
    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();
    const auto  eleRepository  = dbServiceFactory->get<IElementService> ();
    SG::DataStructure::FEM::CWELDData eleData;
    auto      eleSize = eleRepository->Size<SG::DataStructure::FEM::CWELDData>();
    std::vector<int>         dofGlobalIndexSet (12);

    Matrixd kg;
    Real tA{0.0};
    Real tB{0.0};

    for (std::size_t index{ 0 }; index < eleSize; ++index)
    {
        static const DofSet dofSet{ DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz,
                                DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz };

        eleRepository->FindByIndex (index, _OUT eleData);
        assembly.FindNodeIndex (eleData.m_g, _OUT nodeIndexSet);
        assembly.GetGlobalDOFId (nodeIndexSet, dofSet, _OUT dofGlobalIndexSet);

        CweldCalculator cweldcalulator;

        Matrixd globaldisplacement{12, 1};
        for (int i = 0; i < 12; i++)
        {
            globaldisplacement(i, 0) = globalDisplacement[dofGlobalIndexSet[i]];
        }

        //获取得到GA.GB坐标
        SG::Algebra::Matrixd T(3, 3);
        NodeData node;
        Point GA, GB;
        Id dispCoordGA{0};
        Id dispCoordGB{0};

        nodeRepository ->FindById (eleData.m_g[0], _OUT node);
        coordHandler.GetGlobalCoordinate (node, _OUT GA, _OUT dispCoordGA);
        nodeRepository ->FindById (eleData.m_g[1], _OUT node);
        coordHandler.GetGlobalCoordinate (node, _OUT GB, _OUT dispCoordGB);
        //得到局部坐标转换矩阵
        SG::Algebra::computeLocalCoordTrans (GA, GB, _OUT T);
        SG::Algebra::Matrixd TBeam = ElementTools::TwoNodeEleCoordTrans (T, dispCoordGA, dispCoordGB, GA, GB);                                                     
    
// 2. 将节点位移转换到 单元局部坐标系下
        SG::Algebra::Matrixd elementDisplacement = TBeam * globaldisplacement;

// 3. 获取单元在局部坐标系下的梁刚度矩阵
        Real L = length (GA, GB);
        const bool zeroLenCase = (L < 1.0e-6);
        Real Le{0.0};   //等效长度
        // compute beam section property
        PBARData         pBar;
        IsotropicMaterial material;
        {
            PWELDData        pWeld;
            dbServiceFactory->get<IPWELDService> () ->FindById (eleData.m_pId, pWeld);
            dbServiceFactory->get<IIsotropicMaterialService> ()->FindById (pWeld.m_MID, _OUT material);
            
            // create beam section data
            PBARLData    rodSec;
            rodSec.m_id = eleData.m_pId;
            rodSec.m_dimension = {0.5 * pWeld.m_D};
            // 节点重合情况
            if (zeroLenCase)
            {
                // 获取壳截面厚度
                Le = (tA + tB) / 2.0;
            }
            Point centroid;
            auto  calculator = SG::FEMSolver::Element::RodSectionFormulaCalculator ();
            calculator.Compute (rodSec, _OUT pBar, _OUT centroid);
        }
        
        // 粱单元刚度矩阵计算
        Matrix  Te (3, 3);
        if (zeroLenCase)
        {
            // // 梁长为零的特殊情况
            // Point point1{0.0, 0.0, 0.0};
            // Point point2{nA.m_x, nA.m_y, nA.m_z};
            // SG::Algebra::computeLocalCoordTrans (point1, point2, _OUT T);
            // cweldcalulator.ComputeStiffness0 (Le, material, pBar, _OUT kg);
        }
        else
        {
            SG::Algebra::computeLocalCoordTrans (GA, GB, _OUT Te);
            cweldcalulator.ComputeStiffness1 (L, material, pBar, _OUT kg);
        }
        // 4. 利用 $$ F = K U$$计算内力
        elementInternalForce = kg * elementDisplacement;
    }
}

